/*
 * Copyright (c) 2025 STMROS Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "stmconn/stmconn_serial.hpp"
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <cstring>
#include <iostream>

// 简单的消息格式常量
constexpr uint8_t MSG_START_BYTE = 0xAA;
constexpr uint8_t MSG_END_BYTE = 0xBB;
constexpr size_t MAX_PAYLOAD_SIZE = 255;

StmConnSerial::StmConnSerial(const std::string & device, uint32_t baudrate)
  : device_(device), baudrate_(baudrate), serial_fd_(-1), stop_threads_(false) {
}

StmConnSerial::~StmConnSerial() {
  close();
}

bool StmConnSerial::open() {
  // 打开串口设备
  serial_fd_ = ::open(device_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
  if (serial_fd_ < 0) {
    std::cerr << "Failed to open serial device: " << device_ << std::endl;
    return false;
  }
  
  // 配置串口
  if (!configure_serial()) {
    ::close(serial_fd_);
    serial_fd_ = -1;
    return false;
  }
  
  // 启动读写线程
  stop_threads_ = false;
  read_thread_ = std::thread(&StmConnSerial::read_thread_func, this);
  write_thread_ = std::thread(&StmConnSerial::write_thread_func, this);
  
  return true;
}

void StmConnSerial::close() {
  {
    std::lock_guard<std::mutex> lock(mutex_);
    stop_threads_ = true;
    cv_.notify_one();
  }
  
  if (read_thread_.joinable()) {
    read_thread_.join();
  }
  
  if (write_thread_.joinable()) {
    write_thread_.join();
  }
  
  if (serial_fd_ >= 0) {
    ::close(serial_fd_);
    serial_fd_ = -1;
  }
  
  if (closed_cb_) {
    closed_cb_();
  }
}

bool StmConnSerial::is_open() const {
  return serial_fd_ >= 0;
}

bool StmConnSerial::send_message(const stmconn::StmLinkMessage & msg) {
  if (!is_open()) {
    return false;
  }
  
  std::lock_guard<std::mutex> lock(mutex_);
  send_queue_.push(msg);
  cv_.notify_one();
  
  return true;
}

bool StmConnSerial::configure_serial() {
  struct termios options;
  
  // 获取当前配置
  if (tcgetattr(serial_fd_, &options) != 0) {
    std::cerr << "Failed to get serial attributes" << std::endl;
    return false;
  }
  
  // 设置波特率
  speed_t baud_rate;
  switch (baudrate_) {
    case 9600: baud_rate = B9600; break;
    case 19200: baud_rate = B19200; break;
    case 38400: baud_rate = B38400; break;
    case 57600: baud_rate = B57600; break;
    case 115200: baud_rate = B115200; break;
    default: baud_rate = B115200; break;
  }
  
  cfsetispeed(&options, baud_rate);
  cfsetospeed(&options, baud_rate);
  
  // 设置8N1模式
  options.c_cflag &= ~PARENB; // 无校验位
  options.c_cflag &= ~CSTOPB; // 1个停止位
  options.c_cflag &= ~CSIZE;  // 清除数据位设置
  options.c_cflag |= CS8;     // 8个数据位
  
  // 禁用硬件流控制
  options.c_cflag &= ~CRTSCTS;
  
  // 启用接收器，忽略调制解调器控制线
  options.c_cflag |= CREAD | CLOCAL;
  
  // 禁用软件流控制
  options.c_iflag &= ~(IXON | IXOFF | IXANY);
  
  // 原始模式
  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  options.c_oflag &= ~OPOST;
  
  // 设置等待时间和最小接收字符数
  options.c_cc[VMIN] = 1;
  options.c_cc[VTIME] = 0;
  
  // 应用配置
  if (tcsetattr(serial_fd_, TCSANOW, &options) != 0) {
    std::cerr << "Failed to set serial attributes" << std::endl;
    return false;
  }
  
  return true;
}

void StmConnSerial::read_thread_func() {
  uint8_t buffer[4096];
  uint8_t message_buffer[MAX_PAYLOAD_SIZE + 10]; // 额外空间用于头部和尾部
  size_t message_pos = 0;
  bool in_message = false;
  
  while (true) {
    {
      std::lock_guard<std::mutex> lock(mutex_);
      if (stop_threads_) {
        break;
      }
    }
    
    // 读取数据
    ssize_t bytes_read = ::read(serial_fd_, buffer, sizeof(buffer));
    if (bytes_read < 0) {
      if (error_cb_) {
        error_cb_(stmconn::ErrorCode::IO_ERROR);
      }
      continue;
    }
    
    // 解析消息
    for (ssize_t i = 0; i < bytes_read; ++i) {
      uint8_t byte = buffer[i];
      
      if (byte == MSG_START_BYTE) {
        // 开始一个新消息
        in_message = true;
        message_pos = 0;
        message_buffer[message_pos++] = byte;
      } else if (in_message) {
        message_buffer[message_pos++] = byte;
        
        // 检查消息结束
        if (byte == MSG_END_BYTE) {
          stmconn::StmLinkMessage msg;
          if (parse_message(message_buffer, message_pos, msg) && receive_cb_) {
            receive_cb_(msg);
          }
          in_message = false;
        } else if (message_pos >= sizeof(message_buffer)) {
          // 消息过长，丢弃
          in_message = false;
        }
      }
    }
  }
}

void StmConnSerial::write_thread_func() {
  while (true) {
    stmconn::StmLinkMessage msg;
    bool has_message = false;
    
    { // 获取一个消息
      std::unique_lock<std::mutex> lock(mutex_);
      cv_.wait(lock, [this] { return stop_threads_ || !send_queue_.empty(); });
      
      if (stop_threads_ && send_queue_.empty()) {
        break;
      }
      
      if (!send_queue_.empty()) {
        msg = send_queue_.front();
        send_queue_.pop();
        has_message = true;
      }
    }
    
    if (has_message && is_open()) {
      // 简单的消息格式：[START][SYS_ID][COMP_ID][MSG_ID][LENGTH][PAYLOAD][END]
      uint8_t buffer[MAX_PAYLOAD_SIZE + 7]; // 7 bytes for header and footer
      size_t pos = 0;
      
      buffer[pos++] = MSG_START_BYTE;
      buffer[pos++] = msg.system_id;
      buffer[pos++] = msg.component_id;
      buffer[pos++] = (msg.message_id >> 8) & 0xFF;  // MSB
      buffer[pos++] = msg.message_id & 0xFF;         // LSB
      buffer[pos++] = msg.payload.size() & 0xFF;
      
      // 复制负载数据
      if (msg.payload.size() <= MAX_PAYLOAD_SIZE) {
        std::memcpy(buffer + pos, msg.payload.data(), msg.payload.size());
        pos += msg.payload.size();
      }
      
      buffer[pos++] = MSG_END_BYTE;
      
      // 发送数据
      ssize_t bytes_written = ::write(serial_fd_, buffer, pos);
      if (bytes_written < 0 || static_cast<size_t>(bytes_written) != pos) {
        if (error_cb_) {
          error_cb_(stmconn::ErrorCode::IO_ERROR);
        }
      }
    }
  }
}

bool StmConnSerial::parse_message(const uint8_t * data, size_t len, stmconn::StmLinkMessage & msg) {
  // 检查最小长度
  if (len < 7) { // START + SYS_ID + COMP_ID + MSG_ID(2) + LENGTH + END
    return false;
  }
  
  // 检查起始和结束字节
  if (data[0] != MSG_START_BYTE || data[len-1] != MSG_END_BYTE) {
    return false;
  }
  
  // 解析消息头
  msg.system_id = data[1];
  msg.component_id = data[2];
  msg.message_id = (static_cast<uint16_t>(data[3]) << 8) | data[4];
  uint8_t payload_length = data[5];
  
  // 检查负载长度是否匹配
  if (payload_length > 0) {
    if (len != (static_cast<size_t>(7) + payload_length)) {
      return false;
    }
    
    // 复制负载数据
    msg.payload.resize(payload_length);
    std::memcpy(msg.payload.data(), data + 6, payload_length);
  } else {
    msg.payload.clear();
  }
  
  return true;
}